/*
 * rampway.c
 *
 *  Created on: 2024年6月9日
 *      Author: lenovo
 */

#include "zf_common_headfile.h"

enum ramp_type_e ramp_type = RAMP_NONE;

// 圆环状态标志位名称
const char *ramp_type_name[RAMP_NUM] = {
        "RAMP_NONE  ",
        "RAMP_BEGIN ",
        "RAMP_RUNING",
        "RAMP_FALL  "
};

// 记录前方为坡道时的陀螺仪数值
float ramp_last_z = 0.0;
float ramp_last_y = 0.0;
float last_servo_duty = 0.0;
float imu_servo_coefficient = 0.0;

// 菜单控制是否跑坡道
int rampway_flag = 0;

// 检测前方路线是否符合坡道特征
bool check_ramp_line()
{
    if ((Lpt0_found_left == true || Lpt0_found_right == true || Lpt1_found_left == true || Lpt1_found_right == true) \
            || (is_straight0 == false || is_straight1 == false) \
            || (is_bend0 == true || is_bend1 == true))
        return false;
    // 左右边线逐渐离远
    int last_error = 0;
    for (int i = round(aim_distance / sample_dist); i < rptsc0_num * 2 / 3 && i < rptsc1_num * 2 / 3; i++)
    {
        int now_error = rptsc1[i][0] - rptsc0[i][0];
        if (now_error < last_error)
            return false;
        last_error = now_error;
    }

    if (last_error > 10)
        return true;
    else
        return false;
}

// 检测坡道
void check_rampway()
{
    if (!rampway_flag && JOIN_OTHER_STATE && check_ramp_line())
    {
        ramp_last_z = Yaw;
        ramp_last_y = pittch;
        last_servo_duty = servo_duty;
        ramp_type = RAMP_BEGIN;
    }
}

// 跑坡道
void run_ramp()
{
    if (ramp_type == RAMP_BEGIN)
    {
        set_speed = HALF_ADJUST(set_speed, ramp_set_speed_flash);
        // 前方为坡道且车身向上倾,状态改为跑坡道
        if (pittch - ramp_last_y < -6)
            ramp_type = RAMP_RUNING;
    }
    else if (ramp_type == RAMP_RUNING)
    {
        set_speed = ramp_set_speed_flash;
        // 正在跑坡道且车身向下倾,改为下降坡道
        if (pittch - ramp_last_y > 4)
            ramp_type = RAMP_FALL;
    }
    else if (ramp_type == RAMP_FALL)
    {
        set_speed = ramp_set_speed_flash / 2;
        // 在下坡道且车身略回正,结束坡道
        if (pittch - ramp_last_y < 3)
            ramp_type = RAMP_NONE;
    }
    // 方向环由陀螺仪控制
    servo_duty = (Yaw - ramp_last_z) * imu_servo_coefficient + last_servo_duty;
}
